#include "RGBtoYUV.h"

RGBtoYUV::RGBtoYUV()
{
}

RGBtoYUV::~RGBtoYUV()
{
}


// Conver an RGB image into YUV 4-4-4
cv::Mat RGBtoYUV::ConvertRgbToYuv(cv::Mat &img)
{
    cv::Mat yuv411(img.size().height, img.size().width, img.type());

    for (int i = 0; i < img.size().height; ++i)
    {
        for (int j = 0; j < img.size().width; ++j)
        {
            cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
            int B = pixel.val[0];
            int G = pixel.val[1];
            int R = pixel.val[2];

            int Y = ((66 * R + 129 * G + 25 * B + 128) >> 8) + 16;
            int U = ((-38 * R - 74 * G + 112 * B + 128) >> 8)+ 128;
            int V = ((112 * R - 94 * G - 18 * B + 128) >> 8) + 128;

            pixel.val[0] = U;//U;
            pixel.val[1] = Y;//Y;
            pixel.val[2] = V;//V;

            yuv411.at<cv::Vec3b>(i, j) = pixel;
        }
    }
    return yuv411;
}

// Conver an YUV image 4-4-4 into YUV 4-1-1
cv::Mat RGBtoYUV::ConvertYUV_444_To_411(cv::Mat &img)
{
    cv::Mat yuv411(img.size().height, img.size().width, img.type());
    for (int i = 0; i < img.size().height / 2; ++i)
    {
        for (int j = 0; j < img.size().width / 2; ++j)
        {
            cv::Vec3b pixel = img.at<cv::Vec3b>(2 * i, 2 * j);

            cv::Vec3b pixelYuv = yuv411.at<cv::Vec3b>(i, j);

            pixelYuv.val[0] = pixel.val[0];
            pixelYuv.val[2] = pixel.val[2];

            yuv411.at<cv::Vec3b>(i, j) = pixelYuv;
        }
    }
    for (int i = 0; i < img.size().height; ++i)
    {
        for (int j = 0; j < img.size().width; ++j)
        {
            cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
            cv::Vec3b pixelYuv = yuv411.at<cv::Vec3b>(i, j);

            pixelYuv.val[1] = pixel.val[1];
            yuv411.at<cv::Vec3b>(i, j) = pixelYuv;
        }
    }
    return yuv411;
}

cv::Vec3b Pixel411ToRgb(cv::Vec3b pixelYUV, int lumi)
{
    cv::Vec3b pixelRGB;

    int U = pixelYUV.val[0];
    int Y = lumi;//pixelYUV.val[1];
    int V = pixelYUV.val[2];

    int C = Y - 16;
    int D = U - 128;
    int E = V - 128;

    int R = CLIP(( 298 * C + 409 * E + 128) >> 8);
    int G = CLIP(( 298 * C - 100 * D - 208 * E + 128) >> 8);
    int B = CLIP(( 298 * C + 516 * D + 128) >> 8);

    pixelRGB.val[0] = B;
    pixelRGB.val[1] = G;
    pixelRGB.val[2] = R;

    return pixelRGB;
}

//Convert an YUV 4-1-1 image into RGB
cv::Mat  RGBtoYUV::ConvertYuv411ToRgb(cv::Mat &img, int width, int height)
{	
    cv::Mat rgb(img.size().height, img.size().width, img.type());

    for (int i = 0; i < img.size().height; ++i)
    {
        for (int j = 0; j < img.size().width; ++j)
        {
            cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
            cv::Vec3b pixelYuv = rgb.at<cv::Vec3b>(i, j);

            pixelYuv.val[1] = pixel.val[1];
            rgb.at<cv::Vec3b>(i, j) = pixelYuv;
        }
    }

    for (int i = 0; i < img.size().height / 2; ++i)
    {
        for (int j = 0; j < img.size().width /2; ++j)
        {
            cv::Vec3b yuv1 = img.at<cv::Vec3b>(i, j);

            rgb.at<cv::Vec3b>(i*2, j*2) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2, j*2).val[1]);
            rgb.at<cv::Vec3b>(i*2, j*2 + 1) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2, j*2 + 1).val[1]);;
            rgb.at<cv::Vec3b>(i*2 + 1, j*2) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2 + 1, j*2).val[1]);;
            rgb.at<cv::Vec3b>(i*2 + 1, j *2+ 1) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2 + 1, j *2+ 1).val[1]);;
        }
    }

    return rgb;
}


//Convert an YUV 4-4-4 image into RGB
void  RGBtoYUV::ConvertYuvToRgb(cv::Mat &img, int width, int height) const
{	
    for (int i = 0; i < height; ++i)
    {
        for (int j = 0; j < width; ++j)
        {
            cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);

            int U = pixel.val[0];
            int Y = pixel.val[1];
            int V = pixel.val[2];

            int C = Y - 16;
            int D = U - 128;
            int E = V - 128;

            int R = CLIP(( 298 * C + 409 * E + 128) >> 8);
            int G = CLIP(( 298 * C - 100 * D - 208 * E + 128) >> 8);
            int B = CLIP(( 298 * C + 516 * D + 128) >> 8);

            pixel.val[0] = B;
            pixel.val[1] = G;
            pixel.val[2] = R;

            img.at<cv::Vec3b>(i, j) = pixel;
        }
    }
}

